#!/usr/bin/env python
# -*- coding: utf-8 -*-
# thu相对uwb模块编队测试
import os
# import cv2
import time
import numpy as np
import math
# from cir2 import findcir
import rospy
from std_msgs.msg import Float32
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
from std_msgs.msg import Int32
from gazebo_msgs.msg import ModelState
from geometry_msgs.msg import Point
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import Imu
import time
import json



class car_control:
    def __init__(self):
        ############# initialize #############
        self.car_number = rospy.get_param("~car_number", 1)
        self.f = 20.0
        car1_vel, car2_vel =  Twist(), Twist()
        self.kp = 0.8
        self.carpos_x , self.carpos_y = 0.0, 0.0
        self.exp_x , self.exp_y, self.exp_angle = 0.0, 0.0, 0.0
        self.base_dis = 1.0
        self.command = 0
        ############# ros 初始化 #############
        rospy.init_node('car_control', anonymous=True)
        if self.car_number == 1:
            self.car1_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        if self.car_number == 2:
            self.car2_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        rospy.Subscriber('/command', Int32, self.command_callback)
        rospy.Subscriber('/car/position', Pose, self.car_pose_callback, queue_size=1)
        

        rate = rospy.Rate(self.f)
        j = 0
        last_print_t = 0.0
        init_time = rospy.get_time()
        # Main while loop.
        while not rospy.is_shutdown():
            t_loop = rospy.get_time() # time of each loop

            if self.command == 0:
                self.stop()

            elif self.command == 1:
                if self.car_number == 1:
                    self.exp_x = 0.0
                    self.exp_y = 1.0
                    self.exp_angle = 0
                    car1_vel = self.vel_control(self.carpos_x, self.carpos_y, self.exp_x, self.exp_y,self.exp_angle)
                    self.car1_vel_pub.publish(car1_vel)

                if self.car_number == 2:
                    self.exp_x = self.base_dis
                    self.exp_y = 1.0
                    self.exp_angle = 0
                    car2_vel = self.vel_control(self.carpos_x, self.carpos_y, self.exp_x, self.exp_y,self.exp_angle)
                    self.car2_vel_pub.publish(car2_vel)


            elif self.command == 2:
                if self.car_number == 1:
                    self.exp_x = 0.5*self.base_dis
                    self.exp_y = 1.0
                    self.exp_angle = 0
                    car1_vel = self.vel_control(self.carpos_x, self.carpos_y, self.exp_x, self.exp_y,self.exp_angle)
                    self.car1_vel_pub.publish(car1_vel)

                if self.car_number == 2:
                    self.exp_x = 0.5*self.base_dis
                    self.exp_y = 2.0
                    self.exp_angle = 0
                    car2_vel = self.vel_control(self.carpos_x, self.carpos_y, self.exp_x, self.exp_y,self.exp_angle)
                    self.car2_vel_pub.publish(car2_vel)

            elif self.command == 3:
                if self.car_number == 1:
                    self.exp_x = -1.0
                    self.exp_y = 0.0
                    self.exp_angle = 0
                    car1_vel = self.vel_control(self.carpos_x, self.carpos_y, self.exp_x, self.exp_y,self.exp_angle)
                    self.car1_vel_pub.publish(car1_vel)

                if self.car_number == 2:
                    self.exp_x = 2.0
                    self.exp_y = 0.0
                    self.exp_angle = 0
                    car2_vel = self.vel_control(self.carpos_x, self.carpos_y, self.exp_x, self.exp_y,self.exp_angle)
                    self.car2_vel_pub.publish(car2_vel)
                    
            elif self.command == 4:
                if self.car_number == 1:
                    self.exp_x = 0.5*self.base_dis
                    self.exp_y = 0.5*self.base_dis
                    self.exp_angle = 0
                    car1_vel = self.vel_control(self.carpos_x, self.carpos_y, self.exp_x, self.exp_y,self.exp_angle)
                    self.car1_vel_pub.publish(car1_vel)

                if self.car_number == 2:
                    self.exp_x = 0.5*self.base_dis
                    self.exp_y = -0.5*self.base_dis
                    self.exp_angle = 0
                    car2_vel = self.vel_control(self.carpos_x, self.carpos_y, self.exp_x, self.exp_y,self.exp_angle)
                    self.car2_vel_pub.publish(car2_vel)

            # print(car1_vel)
            rate.sleep()


    def vel_control(self, carpos_x, carpos_y, exp_x,exp_y, exp_ang):
        #此处命令系数和小车坐标系和参考系的相对关系有关
        # 105实验：[0 -1; 
        #          1 0]
        vel = Twist()
        vel.linear.x = 0*self.kp*(exp_x-carpos_x) + (-1)*self.kp*(exp_y-carpos_y)
        vel.linear.y = 1*self.kp*(exp_x-carpos_x) + 0*self.kp*(exp_y-carpos_y)
        vel.angular.z = exp_ang
        return vel
    

    def command_callback(self, msg):
        self.command = msg.data
        # print(command)
        print('Receive command  ', msg.data)

    def car_pose_callback(self, msg):
        self.carpos_x = msg.position.x
        self.carpos_y = msg.position.y
        self.base_dis = msg.position.z
        print(self.carpos_x )

    def stop(self):
        if self.car_number == 1:
            self.car1_vel_pub.publish(Twist())
        if self.car_number == 2:
            self.car2_vel_pub.publish(Twist())



if __name__ == '__main__':
    ############# create the log file folder #############

   cc=car_control()
   rospy.spin()

